#ifndef __LEG_H__
#define __LEG_H__

#ifdef __cplusplus
extern "C"
{
#endif

#include "stdio.h"
#include "at32f403a_407.h"



/// Value Limits ///
#define P_MIN -3.141592f
#define P_MAX 3.141592f
#define V_MIN -30.0f
#define V_MAX 30.0f
#define KP_MIN 0.0f
#define KP_MAX 100.0f
#define KD_MIN 0.0f
#define KD_MAX 5.0f
#define T_MIN -4.0f
#define T_MAX 4.0f

/// Joint Soft Stops ///
#define A_LIM_P 1.5f
#define A_LIM_N -1.5f
#define H_LIM_P 5.0f
#define H_LIM_N -5.0f
#define K_LIM_P 0.2f
#define K_LIM_N 7.7f
#define KP_SOFTSTOP 100.0f
#define KD_SOFTSTOP 0.4f;

#define Leg_One 1
#define Leg_Two 2

#define Leg_One_Abad 1
#define Leg_One_Hip 2
#define Leg_One_Knee 3

#define Leg_Two_Abad 4
#define Leg_Two_Hip 5
#define Leg_Two_Knee 6
// 通信板传给VIM3总控的数据
struct spi_data_t
{
    float q_abad[2];
    float q_hip[2];
    float q_knee[2];
    float qd_abad[2];
    float qd_hip[2];
    float qd_knee[2];
    //这里新添加了力矩参数的回传
    float tau_abad[2];
    float tau_hip[2];
    float tau_knee[2];
    float BatorCurrent[2];//这里是电池电压或者是总电流采样
    int32_t flags[2];//标志位，目前做保留没有啥用
    int32_t checksum;//校验位
};


// VIM3传给通信版的的命令
struct spi_command_t
{
    float q_des_abad[2];
    float q_des_hip[2];
    float q_des_knee[2];
    float qd_des_abad[2];
    float qd_des_hip[2];
    float qd_des_knee[2];
    float kp_abad[2];
    float kp_hip[2];
    float kp_knee[2];
    float kd_abad[2];
    float kd_hip[2];
    float kd_knee[2];
    float tau_abad_ff[2];
    float tau_hip_ff[2];
    float tau_knee_ff[2];
    int32_t flags[2];//标志位
    int32_t checksum;//校验位
};


extern struct spi_data_t leg_msg;
extern struct spi_command_t leg_cmd;

#ifdef __cplusplus
}
#endif

#endif